/*PS2手柄初始化定义*/
#include <PS2X_lib.h>  //for v1.6
/******************************************************************
   set pins connected to PS2 controller:
     - 1e column: original
     - 2e colmun: Stef?
   replace pin numbers by the ones you use
 ******************************************************************/
#define PS2_CLK        13  //17
#define PS2_DAT        12  //14    
#define PS2_CMD        11  //15
#define PS2_SEL        10  //16
/******************************************************************
   select modes of PS2 controller:
     - pressures = analog reading of push-butttons
     - rumble    = motor rumbling
   uncomment 1 of the lines for each mode selection
 ******************************************************************/
//#define pressures   true
#define pressures   false
//#define rumble      true
#define rumble      false
PS2X ps2x; // create PS2 Controller Class
//right now, the library does NOT support hot pluggable controllers, meaning
//you must always either restart your Arduino after you connect the controller,
//or call config_gamepad(pins) again after connecting the controller.
int error = 0;
byte type = 0;
byte vibrate = 0;
// Reset func
void (* resetFunc) (void) = 0;
#include <IRremote.h>                   // IRremote库声明 
#include <Servo.h>                       //调用“Servo”库，调制无刷电机PWM输出
Servo ESC;                               //无刷电机命名为“ESC”
String throttle;                         //定义接收字符串为“throttle”
/*红外遥控定义*/
const int RECV_PIN = 3;                  //定义红外接收器的引脚为3
IRrecv irrecv(RECV_PIN);
decode_results results;   //解码结果放在 decode results结构的 result中
/*喇叭定义*/
const int buzzer = 2;                    // 定义喇叭引脚为2号数字引脚
/*前灯定义*/
const int ledPin =  4;                  // 定义灯引脚为4号数字引脚
/*超声波定义*/
unsigned int EchoPin = 5;
unsigned int TrigPin = 6;
 const int light_PIN = 7;                  //定义光照检测器的引脚为6
unsigned long Time_Echo_us = 0;//Len_mm_X100 = length*100
unsigned long Len_mm_X100  = 0;
unsigned long Len_Integer = 0;
unsigned int Time_Echo_us_max = 40000;
unsigned int Time_Echo_us_min = 50;
unsigned int Len_Integer_level = 750;
/*速度定义*/
unsigned int speed_min    = 500;
unsigned int speed_level  = 1150;
unsigned int speed_one    = 1350;
unsigned int speed_two    = 1550;
unsigned int speed_three  = 1750;
unsigned int speed_max    = 1900;
unsigned int speed_add    = 50;
/*心跳包定义*/
int mTime;
bool heart = 0;

void setup()                             //初始化参数
{
  ESC.attach(9);                        // 定义无刷电机引脚为9号数字引脚，可以输出PWM
  Serial.begin(9600);                  //设置串口通讯的波特率为9600
  ESC.writeMicroseconds(1000);          //无刷电机初始为低电平
  pinMode(ledPin, OUTPUT);             //前灯继电器输出
  pinMode(buzzer, OUTPUT);             //喇叭输出
  pinMode(RECV_PIN, INPUT);           //红外接收器输入
  pinMode(EchoPin, INPUT);            //超声波
  pinMode(TrigPin, OUTPUT);
  pinMode(light_PIN, INPUT);             //光照检测器输入
  irrecv.enableIRIn(); // 启动接收器
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
  if (error == 0)
  {
    Serial.print("Found Controller, configured successful ");
    Serial.print("pressures = ");
    if (pressures)
      Serial.println("true ");
    else
      Serial.println("false");
    Serial.print("rumble = ");
    if (rumble)
      Serial.println("true)");
    else
      Serial.println("false");
    Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
    Serial.println("holding L1 or R1 will print out the analog stick values.");
    Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
  }
  else if (error == 1)
    Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
  else if (error == 2)
    Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
  else if (error == 3)
    Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
}
void loop()    //主循环
{
  lanya();     //蓝牙遥控
  delay(10);
  IR();        //红外遥控
  delay(10);
  ps2();      //ps2手柄遥控
  delay(10);
}
